#include <telegram.h>

static uint8_t telegram_ready = 0;
static mavlink_message_t msg;
static mavlink_status_t status;

int telegram_resovle(uint8_t ch)
{
    if (mavlink_parse_char(MAVLINK_COMM_0, ch, &msg, &status) == 1)
    {
        telegram_ready = 1;
        return 0;
    }

    telegram_ready = 0;
    return -1;
}

int telegram_ready_parse()
{
    int rslt = (telegram_ready == 1) ? 0 : -1;
    telegram_ready = 0;
    return rslt;
}

int telegram_read(mavlink_message_t **pmsg, mavlink_status_t **pstatus)
{
    if (pmsg == NULL || pstatus == NULL)
    {
        return -1;
    }

    *pmsg = &msg;
    *pstatus = &status;
    return 0;
}

#include <rtthread.h>
int telegram_print_frame(mavlink_message_t *msg_frame)
{
    if(msg_frame->msgid == MAVLINK_MSG_ID_HEARTBEAT)
    {
        rt_kprintf("mavlink msg heartbeat\n");
    }
    else if(msg_frame->msgid == MAVLINK_MSG_ID_ATTITUDE)
    {
        rt_kprintf("mavlink msg attitude\n");
    }
    return 0;
}
